#include <ros/ros.h>
#include "time.h"
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <sys/time.h>
#include <string>
#include <sstream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <boost/thread/thread.hpp>
#include <unistd.h>

using namespace std;

std::string double2string(double d)
{
     std::ostringstream os;
     os.precision(3);
     os.setf(std::ios::fixed);
     if (os << d)
      return os.str();
     else
      return "invalid conversion";
}
void saveVideo(cv::VideoWriter* writer, vector<cv::Mat>* image_vec, bool* save, bool* record) 
{
    sleep(3);
    cout << image_vec->size() << endl;
    
    
    while(!*record)
    {
       if(image_vec->size() > 0)
       {
	vector<cv::Mat>::iterator it = image_vec->begin();
    	writer->write(*it);
        image_vec->erase(it);
        sleep(0.05);
       }
       else
	continue;
    }
    *save = true;
    
}
int main(int argc , char** argv)
{
    ros::init(argc,argv,"stereo_record");
    ros::NodeHandle nh;
    
    time_t t = time(0);    //获取时间戳
    char time[64];
    strftime(time,sizeof(time),"%Y-%m-%d-%H-%M-%S",localtime(&t));  //将时间戳转换成日期
    std::string time_str = time;
   
    // Enable recording with the filename specified in argument
    std::string videoFileName = time_str + ".avi";
    std::string timeFileName = time_str + "_timeStamp.txt";
    // Start recording, stop with Ctrl-C command
    std::cout <<"Recording " << videoFileName <<",use Ctrl-C to stop." << std::endl;
    cv::Mat srcImage;
    vector<cv::Mat> srcImage_vec;
    struct timeval tm;
    double ms,cs;
    bool finishSave = false;
    bool finishRecord = false;
    cv::VideoCapture cam(1);

    cam.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
    cam.set(CV_CAP_PROP_FPS, 30);
    cam.set(CV_CAP_PROP_FRAME_WIDTH,2560);
    cam.set(CV_CAP_PROP_FRAME_HEIGHT,720);
    double exp = cam.get(CV_CAP_PROP_EXPOSURE);
    double auto_exposure = cam.get(CV_CAP_PROP_AUTO_EXPOSURE);
    cout << "exp: " << exp << " auto exposure: " << auto_exposure << endl;
    cam.set(CV_CAP_PROP_AUTO_EXPOSURE, 0.25); //0.75曝光优先（自动）  0.25快门优先（手动）
    cam.set(CV_CAP_PROP_EXPOSURE,0.1); //在0~1之间，按比例分配 
    
    cv::VideoWriter writer;
    writer.open(videoFileName.c_str(), CV_FOURCC('M', 'J', 'P', 'G'), 30, cv::Size(2560, 720), true);
    if (!cam.isOpened()) 
      exit(0);
    cv::namedWindow("camera",cv::WINDOW_NORMAL);
    cv::resizeWindow("camera",1600,450);
    ofstream ofile;    
    ofile.open(timeFileName.c_str(),ios::out|ios::trunc);    

    boost::thread th1(saveVideo, &writer, &srcImage_vec, &finishSave, &finishRecord);
    int count = 0;
    while (ros::ok()) {
      if(count % 3 == 0)
     {
      cam >> srcImage;
      //writer << srcImage;
      srcImage_vec.push_back(srcImage);
      gettimeofday(&tm, NULL);
      ms = tm.tv_sec + tm.tv_usec / 1000000.0;
      imshow("camera", srcImage);
      ofile << double2string(ms) << "\n";
     }
      cv::waitKey(10);
      count++;
    }
    // Stop recording
    cout << "count is " << count << endl;
    ofile.close();
    cam.release();
    finishRecord = true;
    cout << "Waiting for save the video..." << endl;
    while(!finishSave)
    {
    }
    cout << "Done!!" << endl;
    return 0;
}
